
/**
 * @author Ramon Loureiro
 * @version August 2010
 */


import lejos.nxt.*;

class ControladorVehiculo extends Thread {
   private final double Kp = 1.2;
   private final double Ki = 0.25;
   private final double Kd = 0.1;
   private int n ;

  IntercambiadorDatos myDE ;
  CommonParameters ctrl;
//  ControladorMotores cm ;

  public ControladorVehiculo (CommonParameters ctrl, IntercambiadorDatos DE) {
      this.ctrl = ctrl;
      this.myDE = DE ;
 //     this.cm = new ControladorMotores(Motor.C,Motor.B) ;
      this.n = 0 ;
 }

  private void goForward(){
      myDE.setPower(500) ;
//      cm.goForward(900);
  }

  private void goBackward(){
      myDE.setPower(-200) ;
 //     cm.goForward(-500);
  }

  private void stopVehicle(){
      myDE.setPower(0) ;
//      cm.goForward(0);
  }

  public void run() {

    while (true) {
      try {
        myDE.setContador(n)  ;
        n++ ;

        if (myDE.getTripleDistancia(0)>0) this.goForward();
        if (myDE.getTripleDistancia(1)>0) this.stopVehicle();
        if (myDE.getTripleDistancia(2)>0) this.goBackward();
        /*
        if (n == 100) this.goForward();
        if (n == 200) this.goBackward();
        if (n == 400) this.stopVehicle();

        if (n ==  0) cm.turnCenter();

        if (n ==  50) cm.turnLeft();
        if (n == 150) cm.turnRight();
        if (n == 250) cm.turnCenter();
*/
        if (n > 1000) n = 0 ;
        Thread.sleep(10);
      } catch (InterruptedException ie) {
      }
    }
  }


}